#include "app_cfg.h"
#include "app_type.h"

#include "interfaces.h"
#include "tool/buffer/buffer.h"

#include "framework/vsfsm/vsfsm.h"
#include "framework/vsftimer/vsftimer.h"

#include "sensor.h"

#define SENSOR_IIC_COMPLETE				(VSFSM_EVT_LOCAL + 1)
#define SENSOR_POLL_TICK				(VSFSM_EVT_LOCAL + 2)

#define I2C_XFER(addr, read_buf, write_buf, read_size, wirte_size, read_first) \
	do{sensor->read_len = read_size, sensor->write_len = wirte_size; \
	interfaces->i2c.xfer_nb(sensor->index, addr, read_first, read_buf, read_size, write_buf, wirte_size);}while(0)
#define I2C_READ(addr, buf, size) \
	do{sensor->read_len = size, sensor->write_len = 0; \
	interfaces->i2c.xfer_nb(sensor->index, addr, 1, buf, size, NULL, 0);}while(0)
#define I2C_WRITE(addr, buf, size) \
	do{sensor->read_len = 0, sensor->write_len = size; \
	interfaces->i2c.xfer_nb(sensor->index, addr, 0, NULL, 0, buf, size);}while(0)

static void update_next(struct sensor_t *sensor)
{
	uint8_t *buf = sensor->buf;
	if (sensor->mpu6050.wait_update == 1)
	{
		sensor->i2c_keep = 1;
		sensor->mpu6050.wait_update = 0;
		sensor->mpu6050.i2c_keep = 1;
		buf[0] = 0x3b;
		I2C_XFER(sensor->mpu6050.addr, &sensor->mpu6050.accel_xh, buf, 14, 1, 0);
	}
	else if (sensor->hmc5883l.wait_update == 1)
	{
		sensor->i2c_keep = 1;
		sensor->hmc5883l.wait_update = 0;
		sensor->hmc5883l.i2c_keep = 1;
		buf[0] = 0x2;
		buf[1] = sensor->hmc5883l.mode;
		I2C_XFER(sensor->hmc5883l.addr, &sensor->hmc5883l.x_msb, buf, 6, 2, 0);
	}
	else if (sensor->bmp280.wait_update == 1)
	{
		sensor->i2c_keep = 1;
		sensor->bmp280.wait_update = 0;
		sensor->bmp280.i2c_keep = 1;
		buf[0] = 0xf7;
		I2C_XFER(sensor->bmp280.addr, &sensor->bmp280.press_msb, buf, 6, 1, 0);
	}
	else
	{
		sensor->i2c_keep = 0;
	}
}

static void mpu6050_range_set(struct sensor_t *sensor)
{
	uint8_t *buf = sensor->buf;
	sensor->i2c_keep = 1;
	sensor->mpu6050.range_set = 1;
	sensor->mpu6050.i2c_keep = 1;
	buf[0] = 0x1b;
	buf[1] = sensor->mpu6050.gyro_config;
	buf[2] = sensor->mpu6050.accel_config;
	I2C_WRITE(sensor->mpu6050.addr, buf, 3);
}

static void sensor_i2c_callback_sm(void *param, int32_t read_len,
									int32_t write_len)
{
	struct sensor_t *sensor = param;

	if ((read_len == sensor->read_len) &&
		(write_len == sensor->write_len))
	{
		sensor->error = 0;
	}
	else
	{
		sensor->error = 1;
	}
	vsfsm_post_evt_pending(&sensor->sm, SENSOR_IIC_COMPLETE);
}

#if 0
static void bmp280_update(struct bmp280_t *bmp280)
{
	int32_t t, t_fine, t_var1, t_var2;
	int64_t p, p_var1, p_var2;
	
	t = ((uint32_t)bmp280->temp_msb << 12) +
			((uint32_t)bmp280->temp_lsb << 4) +
			((uint32_t)bmp280->temp_xlsb >> 4);	
	t_var1 = (((t >> 3) - ((int32_t)bmp280->dig_T1 << 1)) *
			(int32_t)bmp280->dig_T2) >> 11;
	t_var2 = (((((t >> 4) - (int32_t)bmp280->dig_T1) * 
			 ((t >> 4) - (int32_t)bmp280->dig_T1)) >> 12) *
		    (int32_t)bmp280->dig_T3) >> 14;
	t_fine = t_var1 + t_var2;
	bmp280->temp = (t_fine * 5 +128) >>8;	
	
	p_var1 = (int64_t)t_fine - 128000;
	p_var2 = p_var1 * p_var1 * (int64_t)bmp280->dig_P6;
	p_var2 = p_var2 + (p_var1 * (int64_t)bmp280->dig_P5 << 17);
	p_var2 = p_var2 + ((int64_t)bmp280->dig_P4 << 35);
	p_var1 = (p_var1 * p_var1 * (int64_t)bmp280->dig_P3 >> 8) +
			 (p_var1 * (int64_t)bmp280->dig_P2 << 12);
	p_var1 = (((int64_t)1 << 47) + p_var1) * (int64_t)bmp280->dig_P1 >> 33;
	if (p_var1 == 0)
	{
		bmp280->press = 0;
	}
	else
	{
		t = ((uint32_t)bmp280->press_msb << 12) +
				((uint32_t)bmp280->press_lsb << 4) +
				((uint32_t)bmp280->press_xlsb >> 4);	
		p = (int64_t)1048576 - t;
		p = ((p << 31) - p_var2) * 3125 / p_var1;
		p_var1 = (int64_t)bmp280->dig_P9 * (p >> 13) * (p >> 13) >> 25;
		p_var2 = (int64_t)bmp280->dig_P8 * p >> 19;
		p = ((p + p_var1 + p_var2) >> 8) + ((int64_t)bmp280->dig_P7 << 4);
		bmp280->press = (int32_t)p;
	}	
}
#else
static void bmp280_update(struct bmp280_t *bmp280)
{
	int32_t t, t_fine, t_var1, t_var2;
	double p, p_var1, p_var2;
	
	t = ((uint32_t)bmp280->temp_msb << 12) +
			((uint32_t)bmp280->temp_lsb << 4) +
			((uint32_t)bmp280->temp_xlsb >> 4);	
	t_var1 = (((t >> 3) - ((int32_t)bmp280->dig_T1 << 1)) *
			(int32_t)bmp280->dig_T2) >> 11;
	t_var2 = (((((t >> 4) - (int32_t)bmp280->dig_T1) * 
			 ((t >> 4) - (int32_t)bmp280->dig_T1)) >> 12) *
		    (int32_t)bmp280->dig_T3) >> 14;
	t_fine = t_var1 + t_var2;
	bmp280->temp = (t_fine * 5 +128) >>8;	
	
	p_var1 = (double)t_fine/2.0 - 64000.0;
	p_var2 = p_var1 * p_var1 * (double)bmp280->dig_P6 / 32768.0;
	p_var2 = p_var2 + p_var1 * (double)bmp280->dig_P5 * 2.0;
	p_var2 = p_var2 / 4.0 + (double)bmp280->dig_P4 * 65536.0;
	p_var1 = (p_var1 * p_var1 * (double)bmp280->dig_P3 / 524288.0 +
				(double)bmp280->dig_P2 * p_var1) / 524288.0;
	p_var1 = (1 + p_var1 / 32768.0) * (double)bmp280->dig_P1;
		
	t = ((uint32_t)bmp280->press_msb << 12) +
			((uint32_t)bmp280->press_lsb << 4) +
			((uint32_t)bmp280->press_xlsb >> 4);	
	
	p = 1048576.0 - t;
	p = (p - p_var2 / 4096.0) * 6250.0 / p_var1;
	p_var1 =  (double)bmp280->dig_P9 * p * p / 2147483648.0;
	p_var2 = (double)bmp280->dig_P8 * p / 32768.0;
	p = p + (p_var1 + p_var2 + (double)bmp280->dig_P7) / 16.0;
	
	bmp280->press = (int32_t)(p * 10.0);	
}
#endif

static void sensor_i2c_callback_poll(void *param, int32_t read_len,
										int32_t write_len)
{
	struct sensor_t *sensor = param;

	if ((read_len != sensor->read_len) || (write_len != sensor->write_len))
	{
		sensor->error = 1;
		sensor->i2c_keep = 0;
		// TODO
		// post error message
		return;
	}

	if (sensor->mpu6050.i2c_keep)
	{
		if (sensor->mpu6050.range_set == 0)
		{
			struct mpu6050_t *mpu6050 = &sensor->mpu6050;
			int16_t tmp;
			uint8_t accel_up = 0, accel_down = 0, gyro_up = 0, gyro_down = 0, range_change = 0;
			uint8_t fs_sel = (mpu6050->gyro_config & 0x18) >> 3;
			uint8_t afs_sel = (mpu6050->accel_config & 0x18) >> 3;
			uint8_t gyro_mult = 0x1 << fs_sel;
			uint8_t accel_mult = 0x1 << afs_sel;			
			
			*(uint16_t *)&tmp = mpu6050->temp_l | ((uint16_t)mpu6050->temp_h << 8);
			mpu6050->temperature = tmp * 10 / 34 + 3653;

			*(uint16_t *)&tmp = mpu6050->accel_xl | ((uint16_t)mpu6050->accel_xh << 8);
			if ((tmp > SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_UPPER) ||
				(tmp < -SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_UPPER))
			{
				accel_up++;
			}
			else if ((tmp < SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_FLOOR) &&
						(tmp > -SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_FLOOR))
			{
				accel_down++;
			}
			mpu6050->accel_x = tmp * accel_mult;

			*(uint16_t *)&tmp = mpu6050->accel_yl | ((uint16_t)mpu6050->accel_yh << 8);
			if ((tmp > SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_UPPER) ||
				(tmp < -SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_UPPER))
			{
				accel_up++;
			}
			else if ((tmp < SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_FLOOR) &&
						(tmp > -SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_FLOOR))
			{
				accel_down++;
			}
			mpu6050->accel_y = tmp * accel_mult;

			*(uint16_t *)&tmp = mpu6050->accel_zl | ((uint16_t)mpu6050->accel_zh << 8);
			if ((tmp > SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_UPPER) ||
				(tmp < -SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_UPPER))
			{
				accel_up++;
			}
			else if ((tmp < SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_FLOOR) &&
						(tmp > -SENSOR_MPU6050_DYNAMIC_RANGE_ACCEL_FLOOR))
			{
				accel_down++;
			}
			mpu6050->accel_z = tmp * accel_mult;

			*(uint16_t *)&tmp = mpu6050->gyro_xl | ((uint16_t)mpu6050->gyro_xh << 8);
			if ((tmp > SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_UPPER) ||
				(tmp < -SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_UPPER))
			{
				gyro_up++;
			}
			else if ((tmp < SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_FLOOR) &&
						(tmp > -SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_FLOOR))
			{
				gyro_down++;
			}
			mpu6050->gyro_x = tmp * gyro_mult;

			*(uint16_t *)&tmp = mpu6050->gyro_yl | ((uint16_t)mpu6050->gyro_yh << 8);
			if ((tmp > SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_UPPER) ||
				(tmp < -SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_UPPER))
			{
				gyro_up++;
			}
			else if ((tmp < SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_FLOOR) &&
						(tmp > -SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_FLOOR))
			{
				gyro_down++;
			}
			mpu6050->gyro_y = tmp * gyro_mult;

			*(uint16_t *)&tmp = mpu6050->gyro_zl | ((uint16_t)mpu6050->gyro_zh << 8);
			if ((tmp > SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_UPPER) ||
				(tmp < -SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_UPPER))
			{
				gyro_up++;
			}
			else if ((tmp < SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_FLOOR) &&
						(tmp > -SENSOR_MPU6050_DYNAMIC_RANGE_GYRO_FLOOR))
			{
				gyro_down++;
			}
			mpu6050->gyro_z = tmp * gyro_mult;

			if (accel_up)
			{
				if (afs_sel < 2)
				{
					afs_sel++;
					mpu6050->accel_config = afs_sel << 3;
					range_change = 1;
				}
			}
			else if (accel_down)
			{
				if (afs_sel > 0)
				{
					afs_sel--;
					mpu6050->accel_config = afs_sel << 3;
					range_change = 1;
				}
			}
			if (gyro_up)
			{
				if (fs_sel < 2)
				{
					fs_sel++;
					mpu6050->gyro_config = fs_sel << 3;
					range_change = 1;
				}
			}
			else if (gyro_down)
			{
				if (fs_sel > 0)
				{
					fs_sel--;
					mpu6050->gyro_config = fs_sel << 3;
					range_change = 1;
				}
			}

			if (range_change)
			{
				mpu6050_range_set(sensor);
				return;
			}
		}
		else
		{
			sensor->mpu6050.range_set = 0;
		}
		sensor->mpu6050.i2c_keep = 0;
	}
	else if(sensor->hmc5883l.i2c_keep)
	{
		sensor->hmc5883l.i2c_keep = 0;
	}
	else if(sensor->bmp280.i2c_keep)
	{
		sensor->bmp280.i2c_keep = 0;		
		bmp280_update(&sensor->bmp280);
	}
	else
	{
		// unknown i2c transfer
		sensor->error = 1;
		sensor->i2c_keep = 0;
		// TODO
		// post error message
		return;
	}

	update_next(sensor);
}

static vsf_err_t sensor_thread(struct vsfsm_pt_t *pt, vsfsm_evt_t evt)
{
	struct sensor_t *sensor = pt->user_data;
	uint8_t *buf = sensor->buf;

	vsfsm_pt_begin(pt);

	/* mpu6050 init */
	// read id
	buf[0] = 0x75;
	I2C_XFER(sensor->mpu6050.addr, &sensor->mpu6050.who_am_i, buf, 1, 1, 0);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	// reset
	buf[0] = 0x6b;
	buf[1] = 0x80;
	I2C_WRITE(sensor->mpu6050.addr, buf, 2);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;
	vsfsm_pt_delay(pt, 100);

	// power on
	buf[0] = 0x6b;
	buf[1] = 0x00;
	I2C_WRITE(sensor->mpu6050.addr, buf, 2);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;
	vsfsm_pt_delay(pt, 10);

	// set clksel
	buf[0] = 0x6b;
	buf[1] = 0x01;
	I2C_WRITE(sensor->mpu6050.addr, buf, 2);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	// set config (refer to inv_mpu6050_init_config() in linux kernel)
	sensor->mpu6050.smplrt_div = 0;			// divide 1
	sensor->mpu6050.config = 1;				// dlpf = 1
	sensor->mpu6050.gyro_config = 3 << 3;	// 2000
	sensor->mpu6050.accel_config = 3 << 3;	// 16
	buf[0] = 0x19;
	buf[1] = sensor->mpu6050.smplrt_div;
	buf[2] = sensor->mpu6050.config;
	buf[3] = sensor->mpu6050.gyro_config;
	buf[4] = sensor->mpu6050.accel_config;
	I2C_WRITE(sensor->mpu6050.addr, buf, 5);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	/* hmc5883l init */
	// read identification
	buf[0] = 0x0a;
	I2C_XFER(sensor->hmc5883l.addr, &sensor->hmc5883l.id_a, buf, 3, 1, 0);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	// set config
	sensor->hmc5883l.config_a = 3 << 5;
	sensor->hmc5883l.config_b = 1 << 5;		// +-1.3Ga, 1090LSB/Ga, [-2048,2047]
	sensor->hmc5883l.mode = 1;				// single-Measurement Mode
	buf[0] = 0x00;
	buf[1] = sensor->hmc5883l.config_a;
	buf[2] = sensor->hmc5883l.config_b;
	buf[3] = sensor->hmc5883l.mode;
	//I2C_WRITE(sensor->hmc5883l.addr, buf, 4);
	I2C_XFER(sensor->hmc5883l.addr, &sensor->hmc5883l.x_msb, buf, 6, 4, 0);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	// ignore measurement check

	/* bmp280 init */
	// read chip id
	buf[0] = 0xd0;
	I2C_XFER(sensor->bmp280.addr, &sensor->bmp280.chip_id, buf, 1, 1, 0);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	// set config
	// normal mode, osrs_p *16, osrs_t *2
	sensor->bmp280.ctrl_meas = 3 | (5 << 2) | (2 << 5);
	// t_sb, filter, spi3w_en 0
	sensor->bmp280.config = 0 | (4 << 2) | (0 << 5);
	buf[0] = 0xf4;
	buf[1] = sensor->bmp280.ctrl_meas;
	buf[2] = sensor->bmp280.config;
	I2C_WRITE(sensor->bmp280.addr, buf, 3);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	// read calibration data
	buf[0] = 0x88;
	I2C_XFER(sensor->bmp280.addr, (uint8_t *)&sensor->bmp280.dig_T1, buf, 24, 1, 0);
	vsfsm_pt_wfe(pt, SENSOR_IIC_COMPLETE);
	if (sensor->error)
		return VSFERR_FAIL;

	/* delay */
	vsfsm_pt_delay(pt, 50);

    /* enable interrupt poll */
	interfaces->i2c.config(sensor->index, sensor->rate, sensor,
							&sensor_i2c_callback_poll);
	sensor->mpu6050.state = SENSOR_STATE_INIT_PASS;
	sensor->hmc5883l.state = SENSOR_STATE_INIT_PASS;
	sensor->bmp280.state = SENSOR_STATE_INIT_PASS;
	sensor->poll = 1;

	vsfsm_pt_end(pt);
	return VSFERR_NONE;
}

static struct vsfsm_state_t *sensor_handler(struct vsfsm_t *sm, vsfsm_evt_t evt)
{
	vsf_err_t err;
	struct sensor_t *sensor = sm->user_data;

	switch (evt)
	{
	case VSFSM_EVT_ENTER:
		break;
	case VSFSM_EVT_INIT:
		interfaces->i2c.init(sensor->index);
		interfaces->i2c.config(sensor->index, sensor->rate, sensor,
								&sensor_i2c_callback_sm);
	default:
		err = sensor->pt.thread(&sensor->pt, evt);
		if (err <= 0)
		{
			// TODO: error deal
		}
		break;
	}

	return NULL;
}

void sensor_init(struct sensor_t *sensor)
{
	sensor->mpu6050.addr = 0x68;
	sensor->hmc5883l.addr = 0x1e;
	sensor->bmp280.addr = 0x76;
	sensor->sm.init_state.evt_handler = sensor_handler;
	sensor->sm.user_data = sensor;
	sensor->pt.sm = &sensor->sm;
	sensor->pt.thread = sensor_thread;
	sensor->pt.user_data = sensor;
	vsfsm_init(&sensor->sm);
}

void sensor_int_callback(struct sensor_t *sensor)
{
	uint32_t tick;

	if (sensor->poll == 0)
		return;

	if (sensor->error)
	{
		// TODO
		// error report
		return;
	}

    if (sensor->i2c_keep == 1)
	{
		sensor->overtime++;
		if (sensor->overtime >= SENSOR_OVERTIME_LIMIT)
		{
			sensor->error = 1;
			// TODO
			// i2c overtime report
		}
		return;
	}
	else
	{
		sensor->overtime = 0;
	}

	// poll interval check
	tick = interfaces->tickclk.get_count();

	if (((sensor->mpu6050.state == SENSOR_STATE_WORKING) ||
			(sensor->mpu6050.state == SENSOR_STATE_INIT_PASS)) &&
		(tick % SENSOR_MPU6050_POLL_INTERVAL == SENSOR_MPU6050_POLL_INTERVAL_OFFSET))
		sensor->mpu6050.wait_update = 1;

	if (((sensor->hmc5883l.state == SENSOR_STATE_WORKING) ||
			(sensor->mpu6050.state == SENSOR_STATE_INIT_PASS)) &&
		(tick % SENSOR_HMC5883L_POLL_INTERVAL == SENSOR_HMC5883L_POLL_INTERVAL_OFFSET))
		sensor->hmc5883l.wait_update = 1;

	if (((sensor->bmp280.state == SENSOR_STATE_WORKING) ||
			(sensor->mpu6050.state == SENSOR_STATE_INIT_PASS)) &&
		(tick % SENSOR_BMP280_POLL_INTERVAL == SENSOR_BMP280_POLL_INTERVAL_OFFSET))
		sensor->bmp280.wait_update = 1;

	update_next(sensor);
}

